#include "StdAfx.h"
#include "SphericalMillingTool.h"
#include <math.h>
#include <assert.h>

SphericalMillingTool::SphericalMillingTool(double _lenght,int _diameter):MillingTool(_lenght,_diameter)
{
}


SphericalMillingTool::~SphericalMillingTool(void)
{
}

double SphericalMillingTool::GetMillingErrorAt(const Point3D & surfacePoint)
{
	double ex = surfacePoint.x - this->posX;
	double ey = surfacePoint.y - this->posY;
	double val = (this->diameter/2)*(this->diameter/2) - ex*ex - ey*ey;
	assert( val >= 0.0 );
	return -(MY_DATATYPE)sqrt(val) + this->posZ  - surfacePoint.z;
}

void SphericalMillingTool::Draw()
{
}

bool SphericalMillingTool::surpassMovementDirectionCond(Point3D &p,const hash_set<int> * visitedPoints)
{
	std::hash_set<int>::iterator it = visitedPoints->find(p.getHashCode2D());
	if(it!=visitedPoints->end())
		return false;
	if(p.x<=(MY_DATATYPE)(this->posX - this->diameter/2) || p.x >= (MY_DATATYPE)(this->posX + this->diameter/2))
		return false;
	if(p.y<=(MY_DATATYPE)(this->posY - this->diameter/2) || p.y >= (MY_DATATYPE)(this->posY + this->diameter/2))
		return false;
	if(pow((double)p.x-this->posX,2) + pow((double)p.y-this->posY,2)>=pow((double)this->diameter/2.0,2))
		return false;
	return true;
}